/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/
#include "StdAfx.h"
#include "Inertia.h"

namespace RobotPlant { namespace Math{
	Inertia::Inertia(void)
	{
		_datas[0][0] = 1; _datas[0][1] = 0; _datas[0][2] = 0;
		_datas[1][0] = 0; _datas[1][1] = 1; _datas[1][2] = 0;
		_datas[2][0] = 0; _datas[2][1] = 0; _datas[2][2] = 1;
	}

	/*Inertia::Inertia(   const double Xx,const double Yx,const double Zx,
						const double Xy,const double Yy,const double Zy,
						const double Xz,const double Yz,const double Zz)
	{
		_datas[0][0] = Xx; _datas[0][1] = Yx; _datas[0][2] = Zx;
		_datas[1][0] = Xy; _datas[1][1] = Yy; _datas[1][2] = Zy;
		_datas[2][0] = Xz; _datas[2][1] = Yz; _datas[2][2] = Zz;
	}

	Inertia::Inertia(const Vector3& x,const Vector3& y,const Vector3& z) 
	{
		_datas[0][0] = x[0]; _datas[0][1] = y[0]; _datas[0][2] = z[0];
		_datas[1][0] = x[1]; _datas[1][1] = y[1]; _datas[1][2] = z[1];
		_datas[2][0] = x[2]; _datas[2][1] = y[2]; _datas[2][2] = z[2];
	}*/

	Inertia::Inertia(const double xx,const double yy,const double zz,
		const double xy,const double yz,const double zx)
	{
		_datas[0][0] = xx; _datas[0][1] = xy; _datas[0][2] = zx;
		_datas[1][0] = xy; _datas[1][1] = yy; _datas[1][2] = yz;
		_datas[2][0] = zx; _datas[2][1] = yz; _datas[2][2] = zz;

	}

	Inertia::~Inertia(void)
	{
	}

	double& Inertia::operator () (const int row,const int column)
	{
		if((row < 0) || (row > 2))
			throw(new std::exception("Argument row index must be between 0 and 2."));
		if((column < 0) || (column > 2))
			throw(new std::exception("Argument column index must be between 0 and 2."));

		return _datas[row][column];
	}

	double Inertia::operator () (const int row,const int column) const
	{
		if((row < 0) || (row > 2))
			throw(new std::exception("Argument row index must be between 0 and 2."));
		if((column < 0) || (column > 2))
			throw(new std::exception("Argument column index must be between 0 and 2."));

		return _datas[row][column];
	}

	Inertia& Inertia::operator = (const Inertia& arg)
	{
		for (int row = 0; row < 3; row++)
		{
			for (int column = 0; column < 3; column++)
			{
				_datas[row][column] = arg._datas[row][column];
			}
		}
		return *this;
	}

	bool operator == (const Inertia& lhs, const Inertia& rhs) 
	{
		return Equal(lhs,rhs);
	}


	bool operator != (const Inertia& lhs, const Inertia& rhs)
	{
		return !Equal(lhs,rhs);
	}

	Vector3 operator * (const Inertia& lhs, const Vector3& rhs)
	{
		// Complexity : 9M+6A
		return Vector3(
			lhs._datas[0][0]*rhs[0] + lhs._datas[0][1]*rhs[1] + lhs._datas[0][2]*rhs[2],
			lhs._datas[1][0]*rhs[0] + lhs._datas[1][1]*rhs[1] + lhs._datas[1][2]*rhs[2],
			lhs._datas[2][0]*rhs[0] + lhs._datas[2][1]*rhs[1] + lhs._datas[2][2]*rhs[2]);
	}

	bool Equal(const Inertia& a,const Inertia& b,const double eps) 
	{
		return (Equal(a._datas[0][0],b._datas[0][0],eps) &&
			Equal(a._datas[0][1],b._datas[0][1],eps) &&
			Equal(a._datas[0][2],b._datas[0][2],eps) &&
			Equal(a._datas[1][0],b._datas[1][0],eps) &&
			Equal(a._datas[1][1],b._datas[1][1],eps) &&
			Equal(a._datas[1][2],b._datas[1][2],eps) &&
			Equal(a._datas[2][0],b._datas[2][0],eps) &&
			Equal(a._datas[2][1],b._datas[2][1],eps) &&
			Equal(a._datas[2][2],b._datas[2][2],eps)    );
	}
}}
